import sensor, image, time, math
from pyb import UART, LED
#LED(1).on()
#LED(2).on()
#LED(3).on()
uart = UART(3, 115200)
sensor.set_hmirror(True)
GRAYSCALE_THRESHOLD = [(0, 25)]
ROIS = [
        (0, 100, 160, 20, 0.7),
        (0,  50, 160, 20, 0.3),
        (0,   0, 160, 20, 0.1)
       ]
weight_sum = 0
for r in ROIS: weight_sum += r[4]
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_hmirror(True)
sensor.set_vflip(True)
clock = time.clock()
while(True):
    clock.tick()
    img = sensor.snapshot()
    centroid_sum = 0
    for r in ROIS:
        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)
        if blobs:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            img.draw_rectangle(largest_blob.rect())
            img.draw_cross(largest_blob.cx(),
                           largest_blob.cy())
            centroid_sum += largest_blob.cx() * r[4]
    center_pos = (centroid_sum / weight_sum)
    deflection_angle = 0
    deflection_angle = -math.atan((center_pos-80)/60)
    deflection_angle = math.degrees(deflection_angle)
    uart.write("%d\n" % deflection_angle)
    print("Turn Angle: %d\n" % deflection_angle)
    time.sleep_ms(100)
